package com.asa.control_theory.utils;

import java.util.ArrayList;
import java.util.List;
import java.util.Map;

import com.asa.control_theory.D;
import com.asa.control_theory.utils.bean.ZhuangTai_Matrix;
import com.asa.control_theory.utils.bean.ZhuangTai_ZhuanYi_Matrix;
import com.asa.hutils.MathHUtils;
import com.asa.utils.Calculate;

/**
 * 
 * 能观性能控性
 * 
 * 能达性等价能控性
 */
public class GongShi6 {

	public static void main(String[] args) throws Exception {
		ZhuangTai_Matrix nengkong1 = new ZhuangTai_Matrix();

//		double[][] ma = { { -2, 0 }, { 0, -1 } };
//		double[][] mb = { { 0 }, { 1 } };
		
//		double[][] ma = { { 1, 1 }, { -2, -1 } };
//		double[][] mb = { { 0 }, { 1 } };
//		
//		
//		double[][] mc = { { 1, 0 } };
//		double[][] md = { { 0 } };
		
		
		
//		double[][] ma = { { 0, 1 ,0}, { 0, 0,1 },{-6,-11,-6} };
//		double[][] mb = { { 0 }, { 0 } , { 1 }};
//		
//		
//		double[][] mc = { { 4,5, 1 } };
//		double[][] md = { { 0 } };
		
		double[][] ma = { { -2, 1}, { 1, -2 }};
		double[][] mb = { { 1 }, { 1 }};
		
		double[][] mc = { { 4,5} };
		double[][] md = { { 0 } };
		
		
		
		nengkong1.setA(ma);
		nengkong1.setB(mb);
		nengkong1.setC(mc);
		nengkong1.setD(md);
		
		double[] juzhengtezhengzhi = MathHUtils.juzhengtezhengzhi(ma);
		System.out.println("特征值：");
		for (int i = 0; i < juzhengtezhengzhi.length; i++) {
			System.out.println(juzhengtezhengzhi[i]);
		}
		
		
		
		//输入能控性
		boolean is_controller = is_controller(nengkong1);
		System.out.println(is_controller);

		
		
//		输出能控性
//		double[][] is_controller_shuchu = is_controller_shuchu(nengkong1);
//		MathHUtils.print(is_controller_shuchu);
		
		
		//能观性
//		double[][] is_observability = is_observability(nengkong1);
//		MathHUtils.print(is_observability);
//		boolean ismangzhi2 = MathHUtils.ismangzhi(is_observability);		
//		System.out.println(ismangzhi2);
		
		
//		double[][] testb = { { 1 }, { 1 } };
//		
//		
//		double[][] testc = { { 0, 1 } };
//		
//		double[][] chenfa = MathHUtils.chenfa(testc, testb);
//		
//		MathHUtils.print(chenfa);
		
	}


	private static boolean is_controller(ZhuangTai_Matrix nengkong1) {
		double[][] ma = nengkong1.getA();
		double[][] mb = nengkong1.getB();
		double[][] ctrb = D.ctrb(ma, mb);
		MathHUtils.print(ctrb);
//		boolean isrankall = D.isrankall(ctrb);
		boolean ismangzhi = MathHUtils.ismangzhi(ctrb);		
		
//		System.out.println(ismangzhi);
		return ismangzhi;
	}


	/**
	 * 能观性矩阵计算
	 * @param nengkong1
	 * @return
	 */
	public static double[][] is_observability(ZhuangTai_Matrix nengkong1) {
		
		double[][] ma = nengkong1.getA();
		double[][] mb = nengkong1.getB();
		double[][] mc = nengkong1.getC();
		double[][] md = nengkong1.getD();
		
		
		double[][][] last = new double[ma.length][ma.length][ma.length];
		last[0]=MathHUtils.getI(ma.length);
		for (int i = 1; i < last.length; i++) {
			last[i] = MathHUtils.chenfa(last[i-1], ma);
		}
//		MathHUtils.print(last[last.length-1]);
		
		
		double[][] CT = MathHUtils.zhuanzhi(mc);
		
		List<double[]> result_list = new ArrayList();
		for (int i = 0; i < last.length; i++) {
			double[][] ds = last[i];
			double[][] zhuanzhi = MathHUtils.zhuanzhi(ds);
			
			double[][] chenfa = MathHUtils.chenfa(zhuanzhi, CT);
			chenfa = MathHUtils.zhuanzhi(chenfa);

			for (int j = 0; j < chenfa.length; j++) {
				result_list.add(chenfa[j]);
			}
		}
		
		double[][] result = new double[result_list.size()][];
		
		for (int i = 0; i < result.length; i++) {
			result[i] = result_list.get(i);
		}
		
		result = MathHUtils.zhuanzhi(result);

		
		return result;

	}
	
	
	
	
	





	/**
	 * 输出能控性，能控性矩阵计算
	 * @param nengkong1
	 * @return
	 */
	public static double[][] is_controller_shuchu(ZhuangTai_Matrix nengkong1) {
		
		double[][] ma = nengkong1.getA();
		double[][] mb = nengkong1.getB();
		double[][] mc = nengkong1.getC();
		double[][] md = nengkong1.getD();
		
		
		//空间换时间，计算A^n
		double[][][] last = new double[ma.length][ma.length][ma.length];
		last[0]=MathHUtils.getI(ma.length);
		for (int i = 1; i < last.length; i++) {
			last[i] = MathHUtils.chenfa(last[i-1], ma);
		}
		MathHUtils.print(last[last.length-1]);
		
		
		double[][][] result = new double[ma.length+1][][];
		for (int i = 0; i < result.length-1; i++) {
			
			
			double[][] ds = last[i];
			double[][] chenfa2 = MathHUtils.chenfa(mc, ds);
			double[][] chenfa3 = MathHUtils.chenfa(chenfa2, mb);
			
			result[i] = chenfa3;
			
			
		}
		result[result.length-1] = md;
		
		double[][] jisuan = null;
		for (int i = 0; i < result.length; i++) {
			System.out.println("===========");
			double[][] ds = result[i];
			
			
			
			if (jisuan==null) {
				jisuan = ds;
			}else {

				
				jisuan = MathHUtils.add_hang(jisuan, ds, jisuan.length);

			}
			
			
		}
		System.out.println("需要计算的东西asa");
		MathHUtils.print(jisuan);
		System.out.println("----------------");
		
//		D.isrankall(jisuan);
		
		
		
		//我不知道怎么判断这里result的秩是多少
		boolean ismangzhi = MathHUtils.ismangzhi(jisuan);
		if (!ismangzhi) {
			System.out.println("非满秩，不能控");
		}else {
			System.out.println("满了");
		}
		
		
		
		
		
		return jisuan;
		
	}

	
	
	
	
	


	/**
	 * 输入能控性，这种方式是把状态矩阵转化成对角线标准型，然后再判断的
	 * 判断是否是能控的，方法思路是将状态矩阵转化成对角线标准型
	 * 然后判断B上对应的特征值的行是否为0，存在为0的就不可控
	 * 如果是重根的情况，就是判断约旦标准型上，重的最大的重数对应的B的行上的值是否都为0
	 * 这是一种方法，还有一种D.java中有方法，A和B相乘，根据是否满秩来判断
	 * @param nengkong1
	 * @param ma
	 * @param mb
	 * @param mc
	 * @param md
	 * @return
	 * @throws Exception
	 */
	public static boolean is_controller_2duijiaojuzheng(ZhuangTai_Matrix nengkong1) throws Exception {
		double[] juzhengtezhengzhi = MathHUtils.juzhengtezhengzhi(nengkong1.getA());

		for (int i = 0; i < juzhengtezhengzhi.length; i++) {
			System.out.println(juzhengtezhengzhi[i] + "................................asa");
		}

		



		System.out.println(",..................");
		ZhuangTai_Matrix duijiaoxian2 = GongShi2.duijiaoxian(nengkong1);
		GongShi.print(duijiaoxian2);
		
		
		
		Map<Integer, Integer> weizhi_dijichonggen = MatrixUtils.weizhi_dijichonggen(juzhengtezhengzhi);
		double[][] b = nengkong1.getB();
		for (int i = 0; i < b.length; i++) {
			if (weizhi_dijichonggen.get(i)==null) {

				if (i<b.length-1) {
					if (weizhi_dijichonggen.get(i+1)!=null&&weizhi_dijichonggen.get(i)>weizhi_dijichonggen.get(i+1)) {
						continue;
					}
				}
			}
			
			
			double[] ds = b[i];
			boolean isall0 = isall0(ds);
//			System.out.println(ds[0]+"......................."+i);
			if (isall0) {
				return false;
			}
			
		}
		
		return true;
	}
	
	/**
	 * 能观性，这种方式是把状态矩阵转化成对角线标准型，然后再判断的
	 * @param nengkong1
	 * @return
	 * @throws Exception
	 */
	public static boolean is_observability_2duijiaojuzheng(ZhuangTai_Matrix nengkong1) throws Exception {
		
		
		double[] juzhengtezhengzhi = MathHUtils.juzhengtezhengzhi(nengkong1.getA());

		for (int i = 0; i < juzhengtezhengzhi.length; i++) {
			System.out.println(juzhengtezhengzhi[i] + "................................asa");
		}


		System.out.println(",..................");
		ZhuangTai_Matrix duijiaoxian2 = GongShi2.duijiaoxian(nengkong1);
		GongShi.print(duijiaoxian2);
		
		Map<Integer, Integer> weizhi_dijichonggen = MatrixUtils.weizhi_dijichonggen(juzhengtezhengzhi);
		double[][] b = nengkong1.getB();
		
		
		
		for (int i = 0; i < b[0].length; i++) {
			
			 Integer integer = weizhi_dijichonggen.get(i);
	            
	         if (integer==null) {//等于null，就意味着这个位置不是重根或者是重根的第一位置
				double[] asa = new double[b.length];
	        	 for (int j = 0; j < b.length; j++) {
	        		 asa[j] = b[j][i];
				}
	        	 
	        	 
	        	 boolean isall0 = isall0(asa);
	        	 
	        	 if (isall0) {
					return false;
				}
	        	 
			 }
			
			
			
		}
		
            
            
           
            
            
        
		
		
		
		
		
		
		
		return false;
		
		

		
		
		
	}

	
	
	
	
	
	
	
	
	
	
	
	private static boolean isall0(double[] asa) {
		
		
		boolean result = true;
		
		
		for (int i = 0; i < asa.length; i++) {
			if (Math.abs(asa[i])>=MathHUtils.to0) {
//			if (Math.abs(asa[i])==0) {
				result = false;
				break;
			}
		}
		
		
		return result;
		
	}
	
	
	
	
	
	
	
	
	
	

}
